#include "pod.h"
#include <iostream>

bool Pod::running = false;
TStateMachineState Pod::state = smsUNKNOWN;
BYTE Pod::state_changed = 0;
float Pod::x = 0;
float Pod::y = 0;
float Pod::z = 0;
float Pod::roll = 0;
float Pod::pitch = 0;
float Pod::yaw = 0;
int Pod::inputs[NUM_INPUTS];

Pod::Pod(void) {
   //std::cout << "Initialising motion pod.\n";
   //read platform parameters
   std::cout << "Reading platform parameters.\n";
   TPPFError ppferror = readPlatformParameterFile(fpPLATFORM_PARAMETERS, NULL, &platform);
   if(ppferror != ppfSUCCESS) {
      std::cerr << "Error: Could not read " << fpPLATFORM_PARAMETERS << ".\n";
      std::cerr << "       " << ppfErrorMessage(ppferror) << ".\n";
      exit(1);
   }
   //read position parameters
   std::cout << "Reading position parameters.\n";
   TPOSPFError pospferror = readPositionParameterFile(fpPOSITION_PARAMETERS, NULL, &positions);
   if(pospferror != pospfSUCCESS) {
      std::cerr << "Error: Could not read " << fpPOSITION_PARAMETERS << ".\n";
      std::cerr << "       " << pospfErrorMessage(pospferror) << ".\n";
      exit(1);
   }
   //read filter parameters
   std::cout << "Reading filter parameters.\n";
   TFPFError fpferror = readFilterParameterFile(fpFILTER_PARAMETERS, NULL, &filters);
   if(fpferror != fpfSUCCESS) {;
      std::cerr << "Error: Could not read " << fpFILTER_PARAMETERS << ".\n";
      std::cerr << "       " << fpfErrorMessage(fpferror) << ".\n";
      exit(1);
   }

   //std::cout << "Motion pod ready.\n";
}

unsigned int __stdcall Pod::run(void* a) 
{
   static clock_t start = clock();
   double interval = INTERVAL * (double)CLOCKS_PER_SEC;
   clock_t goal;
   int input_no;
   while(running) {
      if(serviceStateMachine(&state, &state_changed)) {
         if(state_changed) {
            std::cout << stateDescriptor(state) << "\n";
         }
         switch(state) {
            case smsPSU_DISABLED:
               if(state_changed) {
                  setPlatformCommand(pcENABLE_PSU);
               }
               break;
            case smsIDLE:
                  setPlatformCommand(pcFOLLOW);
               break;
            case smsFOLLOWING:
               //update pod position
               setHostDataLatches(x, y, z, roll, pitch, yaw, -1);
               break;
         }
         //poll inputs, analog inputs
         for(input_no = 0; input_no < NUM_ANALOG_INPUTS; ++input_no) {
            getAnalogInput(input_no, &inputs[input_no]);
         }
         // digital inputs
         BYTE digital_input;
         for(input_no = 0; input_no < NUM_DIGITAL_INPUTS; ++input_no) { 
            getDigitalInput(DIGITAL_OFFSET + input_no, &digital_input);
            inputs[input_no + NUM_ANALOG_INPUTS] = (int) digital_input;
         }
      }
      //sleep until end of real time interval
      //goal = clock() + RT_INTERVAL - rt_clock;
      goal = start + (clock_t)interval;
      while(clock() < goal) Sleep(1);
      //rt_clock = clock();
      start = clock();
   }
   _endthreadex(NULL);
   return 0;
}

void Pod::start(void) {
   if(!running) {
      //initialise state machine
      std::cout << "Initialising state machine.\n";
      if(initialiseStateMachine(adrMCC, fpMAIN, &platform, platform.mdu.stateMachinePeriod) != erSUCCESS) {
         std::cerr << "Error: Could not initialise state machine.\n";
         exit(1);
      }
      //install desired kinematics handler
      std::cout << "Installing kinematics handler.\n";
      setKinematicsType(ktSTEWART_PLATFORM);
      //download parked and neutral positions
      std::cout << "Downloading parked position.\n";
      setParkedPosition(positions.parkedPosition);
      std::cout << "Downloading neutral position.\n";
      setNeutralPosition(positions.neutralPosition);     
      //initialise filters
      std::cout << "Initialising filters.\n";
      for(int f = 0; f < cnNUMBER_OF_FILTER_BLOCKS; f++) {
         if(setFilterBlock(f, &filters.filterBlock[f], platform.mdu.stateMachinePeriod) != erSUCCESS) {
               std::cerr << "Error: Could not set filter block " << f << ".\n";
               exit(1);
         }
      }
      //enable motion cueing algorithms (if enabled)
      std::cout << "Initialising motion cueing algorithms.\n";
      if(setMotionCueing(filters.motionCueingOn, filters.specificForcesOn, filters.adaptionOn) != erSUCCESS) {
            std::cerr << "Error could not set motion cueing.\n";
            exit(1);
      }
      running = true;
      _beginthreadex(NULL, 0, &run, NULL, 0, NULL);
   }
}

void Pod::stop(void) {
   if(running) {
      setPlatformCommand(pcSTOP);
      running = false;
      printf("Shutting down state machine.\n");
      shutdownStateMachine();
   } else {
      printf("Cannot stop, the pod was not running\n");
   }
}

void Pod::home(void) {
   //setPlatformCommand(pcHOME);
   x = 0;
   y = 0;
   z = 0;
   roll = 0;
   pitch = 0;
   yaw = 0;

}

void Pod::park(void) {
   setPlatformCommand(pcGO_TO_PARKED);
}

void Pod::follow(void) {
   if(state == smsIDLE) {
      setPlatformCommand(pcFOLLOW);
   }
}

const char* Pod::get_state(void) {
   return stateDescriptor(state);
}

